//#include <d3dx9.h>
#include "../../include/DirectX9/d3dx9.h"
#include <math.h>

#if defined(__GNUC__) && defined(__i386__)
inline void sincosf(float angle, float &sin, float &cos)
{
	asm("fsincos" : "=t" (cos), "=u" (sin) : "0" (angle));
}
#else
inline void sincosf(float angle, float &sin, float &cos)
{
	sin = ::sin(angle);
	cos = ::cos(angle);
}
#endif

extern "C"
{

/*
 * D3DXMatrixAffineTransformation
 *
 * Builds an affine transformation matrix.
 * 
 * Parameters
 *    pOut
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    scaling
 *       Scaling factor. 
 *    pRotationCenter
 *       Pointer to a D3DXVECTOR3 structure, a point identifying the center
 *       of rotation. If this argument is NULL, it is treated as identity. 
 *    pRotation 
 *       Pointer to a D3DXQUATERNION structure that specifies the rotation.
 *       If this argument is NULL, it is treated as identity.
 *    pTranslation 
 *       Pointer to a D3DXVECTOR3 structure, representing the translation.
 *       If this argument is NULL, it is treated as identity. 
 * 
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is an affine transformation
 *    matrix.
 * 
 * Remarks
 *    The D3DXMatrixAffineTransformation function calculates the affine
 *    transformation matrix with the following formula:
 *    Ms * Mrc-1 * Mr * Mrc * Mt, where Ms is the scaling matrix,
 *    Mrc is the center of rotation matrix, Mr is the rotation matrix,
 *    and Mt is the translation matrix.
 * 
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixAffineTransformation
 *    function can be used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixAffineTransformation(D3DXMATRIX* pOut,
	float scaling, CONST D3DXVECTOR3* pRotationCenter,
	CONST D3DXQUATERNION* pRotation, CONST D3DXVECTOR3* pTranslation)
{
	D3DXMATRIX Result;

	Result._12 = Result._13 = Result._14 = 0.0f; Result._11 = scaling;
	Result._21 = Result._23 = Result._24 = 0.0f; Result._22 = scaling;
	Result._31 = Result._32 = Result._34 = 0.0f; Result._33 = scaling;
	if (pTranslation)
	{
		Result._41 = pTranslation->x; Result._42 = pTranslation->y;
		Result._43 = pTranslation->z; Result._44 = 1.0f; 
	} else
	{
		Result._41 = Result._42 = Result._43 = 0.0f; Result._44 = 1.0f; 
	}
	if (pRotation)
	{
		D3DXMATRIX Temp;

		if (pRotationCenter)
		{
			Temp._41 -= pRotationCenter->x;
			Temp._42 -= pRotationCenter->y;
			Temp._43 -= pRotationCenter->z;
		}
		Result *= *D3DXMatrixRotationQuaternion(&Temp, pRotation);
		if (pRotationCenter)
			Result *= *D3DXMatrixTranslation(&Temp, pRotationCenter->x,
				pRotationCenter->y, pRotationCenter->z);
	}
    *pOut = Result;

	return pOut;
}

/*
 * D3DXMatrixDeterminant
 *
 * Returns the determinant of a matrix.
 *
 * Parameters
 *    pM
 *       Pointer to the source D3DXMATRIX structure.
 *
 * Return Values
 *    The value of the matrix, the determinant.
 *
 * Status
 *    Works
 *
 */

float WINAPI D3DXMatrixDeterminant(CONST D3DXMATRIX *pM)
{
	return (
		+ pM->m[0][0] * pM->m[1][1] * pM->m[2][2] * pM->m[3][3]
		- pM->m[0][0] * pM->m[1][1] * pM->m[2][3] * pM->m[3][2]
		- pM->m[0][0] * pM->m[2][1] * pM->m[1][2] * pM->m[3][3]
		+ pM->m[0][0] * pM->m[2][1] * pM->m[1][3] * pM->m[3][2]
		+ pM->m[0][0] * pM->m[3][1] * pM->m[1][2] * pM->m[2][3]
		- pM->m[0][0] * pM->m[3][1] * pM->m[1][3] * pM->m[2][2]
		- pM->m[1][0] * pM->m[0][1] * pM->m[2][2] * pM->m[3][3]
		+ pM->m[1][0] * pM->m[0][1] * pM->m[2][3] * pM->m[3][2]
		+ pM->m[1][0] * pM->m[2][1] * pM->m[0][2] * pM->m[3][3]
		- pM->m[1][0] * pM->m[2][1] * pM->m[0][3] * pM->m[3][2]
		- pM->m[1][0] * pM->m[3][1] * pM->m[0][2] * pM->m[2][3]
		+ pM->m[1][0] * pM->m[3][1] * pM->m[0][3] * pM->m[2][2]
		+ pM->m[2][0] * pM->m[0][1] * pM->m[1][2] * pM->m[3][3]
		- pM->m[2][0] * pM->m[0][1] * pM->m[1][3] * pM->m[3][2]
		- pM->m[2][0] * pM->m[1][1] * pM->m[0][2] * pM->m[3][3]
		+ pM->m[2][0] * pM->m[1][1] * pM->m[0][3] * pM->m[3][2]
		+ pM->m[2][0] * pM->m[3][1] * pM->m[0][2] * pM->m[1][3]
		- pM->m[2][0] * pM->m[3][1] * pM->m[0][3] * pM->m[1][2]
		- pM->m[3][0] * pM->m[0][1] * pM->m[1][2] * pM->m[2][3]
		+ pM->m[3][0] * pM->m[0][1] * pM->m[1][3] * pM->m[2][2]
		+ pM->m[3][0] * pM->m[1][1] * pM->m[0][2] * pM->m[2][3]
		- pM->m[3][0] * pM->m[1][1] * pM->m[0][3] * pM->m[2][2]
		- pM->m[3][0] * pM->m[2][1] * pM->m[0][2] * pM->m[1][3]
		+ pM->m[3][0] * pM->m[2][1] * pM->m[0][3] * pM->m[1][2]
	);
}

/*
 * D3DXMatrixInverse
 *
 * Calculates the inverse of a matrix.
 *
 * Parameters
 *    pOut
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pDeterminant 
 *       After this function returns, pointer to a float value that is
 *       the determinant of the matrix, pM. 
 *    pM 
 *       Pointer to the source D3DXMATRIX structure. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is the inverse of the matrix.
 *
 * Remarks
 *    This function accepts any arbitrary matrix.
 *
 *    Matrix inversion may fail, in which case NULL is returned by the
 *    D3DXMatrixInverse function. The determinant of pM is returned if
 *    the pDeterminant parameter is non-NULL.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixInverse function can be
 *    used as a parameter for another function.
 *
 * Status
 *    Untested
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixInverse(D3DXMATRIX* pOut, float* pDeterminant,
	CONST D3DXMATRIX* pM)
{
	float fDetInv;

	if (fabs(pM->_44 - 1.0f) > .001f)
		return NULL;
	if (fabs(pM->_14) > .001f || fabs(pM->_24) > .001f || fabs(pM->_34) > .001f)
		return NULL;

	if (pDeterminant != NULL)
	{
	    *pDeterminant = D3DXMatrixDeterminant(pM);
		fDetInv = 1.0f / *pDeterminant;
	} else
		fDetInv = 1.0f / D3DXMatrixDeterminant(pM);

	pOut->_11 = fDetInv * (pM->_22 * pM->_33 - pM->_23 * pM->_32);
	pOut->_12 = -fDetInv * (pM->_12 * pM->_33 - pM->_13 * pM->_32);
	pOut->_13 = fDetInv * (pM->_12 * pM->_23 - pM->_13 * pM->_22);
	pOut->_14 = 0.0f;
	pOut->_21 = -fDetInv * (pM->_21 * pM->_33 - pM->_23 * pM->_31);
	pOut->_22 = fDetInv * (pM->_11 * pM->_33 - pM->_13 * pM->_31);
	pOut->_23 = -fDetInv * (pM->_11 * pM->_23 - pM->_13 * pM->_21);
	pOut->_24 = 0.0f;
	pOut->_31 = fDetInv * (pM->_21 * pM->_32 - pM->_22 * pM->_31);
	pOut->_32 = -fDetInv * (pM->_11 * pM->_32 - pM->_12 * pM->_31);
	pOut->_33 = fDetInv * (pM->_11 * pM->_22 - pM->_12 * pM->_21);
	pOut->_34 = 0.0f;
	pOut->_41 = -(pM->_41 * pOut->_11 + pM->_42 * pOut->_21 + pM->_43 * pOut->_31);
	pOut->_42 = -(pM->_41 * pOut->_12 + pM->_42 * pOut->_22 + pM->_43 * pOut->_32);
	pOut->_43 = -(pM->_41 * pOut->_13 + pM->_42 * pOut->_23 + pM->_43 * pOut->_33);
	pOut->_44 = 1.0f;

	return pOut;	
}

/*
 * D3DXMatrixLookAtLH
 *
 * Builds a left-handed, look-at matrix.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pEye 
 *       Pointer to the D3DXVECTOR3 structure that defines the eye point.
 *       This value is used in translation. 
 *    pAt 
 *       Pointer to the D3DXVECTOR3 structure that defines the camera
 *       look-at target. 
 *    pUp 
 *       Pointer to the D3DXVECTOR3 structure that defines the current
 *       world's up, usually [0, 1, 0]. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a left-handed, look-at matrix.
 *
 * Remarks
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixLookAtLH function can be
 *    used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixLookAtLH(D3DXMATRIX* pOut, CONST D3DXVECTOR3* pEye,
	CONST D3DXVECTOR3* pAt, CONST D3DXVECTOR3* pUp)
{
	D3DXVECTOR3 XAxis, YAxis, ZAxis;

	// Get the z basis vector, which points straight ahead; the
	// difference from the eye point to the look-at point. This is the 
	// direction of the gaze (+z).
    D3DXVec3Subtract(&ZAxis, pAt, pEye);

	// Normalize the z basis vector.
	D3DXVec3Normalize(&ZAxis, &ZAxis);

	// Compute the orthogonal axes from the cross product of the gaze 
	// and the pUp vector.
	D3DXVec3Cross(&XAxis, pUp, &ZAxis);
	D3DXVec3Normalize(&XAxis, &XAxis);
	D3DXVec3Cross(&YAxis, &ZAxis, &XAxis);

	// Start building the matrix. The first three rows contain the 
	// basis vectors used to rotate the view to point at the look-at 
	// point. The fourth row contains the translation values. 
	// Rotations are still about the eyepoint.
	pOut->_11 = XAxis.x;
	pOut->_21 = XAxis.y;
	pOut->_31 = XAxis.z;
	pOut->_41 = -D3DXVec3Dot(&XAxis, pEye);

	pOut->_12 = YAxis.x;
	pOut->_22 = YAxis.y;
	pOut->_32 = YAxis.z;
	pOut->_42 = -D3DXVec3Dot(&YAxis, pEye);

	pOut->_13 = ZAxis.x;
	pOut->_23 = ZAxis.y;
	pOut->_33 = ZAxis.z;
	pOut->_43 = -D3DXVec3Dot(&ZAxis, pEye);

	pOut->_14 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_34 = 0.0f;
	pOut->_44 = 1.0f;

	return pOut;
}

/*
 * D3DXMatrixLookAtRH
 *
 * Builds a right-handed, look-at matrix.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pEye 
 *       Pointer to the D3DXVECTOR3 structure that defines the eye point.
 *       This value is used in translation. 
 *    pAt 
 *       Pointer to the D3DXVECTOR3 structure that defines the camera
 *       look-at target. 
 *    pUp 
 *       Pointer to the D3DXVECTOR3 structure that defines the current
 *       world's up, usually [0, 1, 0]. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a right-handed, look-at matrix.
 *
 * Remarks
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixLookAtRH function can be
 *    used as a parameter for another function.
 *
 * Status
 *    Untested
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixLookAtRH(D3DXMATRIX* pOut, CONST D3DXVECTOR3* pEye,
	CONST D3DXVECTOR3* pAt, CONST D3DXVECTOR3* pUp)
{
	D3DXVECTOR3 XAxis, YAxis, ZAxis;

	// Get the z basis vector, which points straight ahead; the
	// difference from the eye point to the look-at point. This is the 
	// direction of the gaze (+z).
    D3DXVec3Subtract(&ZAxis, pAt, pEye);

	// Normalize the z basis vector.
	D3DXVec3Normalize(&ZAxis, &ZAxis);

	// Compute the orthogonal axes from the cross product of the gaze 
	// and the pUp vector.
	D3DXVec3Cross(&XAxis, pUp, &ZAxis);
	D3DXVec3Normalize(&XAxis, &XAxis);
	D3DXVec3Cross(&YAxis, &ZAxis, &XAxis);

	// Start building the matrix. The first three rows contain the 
	// basis vectors used to rotate the view to point at the look-at 
	// point. The fourth row contains the translation values. 
	// Rotations are still about the eyepoint.
	pOut->_11 = -XAxis.x;
	pOut->_21 = -XAxis.y;
	pOut->_31 = -XAxis.z;
	pOut->_41 = D3DXVec3Dot(&XAxis, pEye);

	pOut->_12 = YAxis.x;
	pOut->_22 = YAxis.y;
	pOut->_32 = YAxis.z;
	pOut->_42 = -D3DXVec3Dot(&YAxis, pEye);

	pOut->_13 = -ZAxis.x;
	pOut->_23 = -ZAxis.y;
	pOut->_33 = -ZAxis.z;
	pOut->_43 = D3DXVec3Dot(&ZAxis, pEye);

	pOut->_14 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_34 = 0.0f;
	pOut->_44 = 1.0f;

	return pOut;
}

/*
 * D3DXMatrixMultiply
 *
 * Determines the product of two matrices.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pM1 
 *       Pointer to a source D3DXMATRIX structure. 
 *    pM2 
 *       Pointer to a source D3DXMATRIX structure. 
 * 
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is the product of two matrices.
 * 
 * Remarks
 *    The result represents the transformation M2 followed by the
 *    transformation M1 (Out = M1 * M2).
 * 
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixMultiply function can be
 *    used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixMultiply(D3DXMATRIX* pOut, const D3DXMATRIX* pM1,
	const D3DXMATRIX* pM2)
{
	float* pA = (float*)pM1;
	float* pB = (float*)pM2;
	float pM[16];

	ZeroMemory(pM, sizeof(D3DMATRIX));
	for (WORD i = 0; i < 4; i++)
		for (WORD j = 0; j < 4; j++) 
			for (WORD k = 0; k < 4; k++) 
				pM[4 * i + j] += pA[4 * i + k] * pB[4 * k + j];
	memcpy(pOut, pM, sizeof(D3DMATRIX));
	return pOut;
}

/*
 * D3DXMatrixMultiplyTranspose
 *
 * Determines the product of two matrices, followed by a transpose.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pM1 
 *       Pointer to a source D3DXMATRIX structure. 
 *    pM2 
 *       Pointer to a source D3DXMATRIX structure. 
 * 
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is the product of two matrices.
 * 
 * Remarks
 *    The result represents the transformation M2, followed by the
 *    transformation M1, tranposed by T (Out = T(M1 * M2)).
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixMultiplyTranspose function
 *    can be used as a parameter for another function.
 *
 *    This function is useful to set matrices as constants for vertex and
 *    pixel shaders.
 *
 * Status
 *    Untested
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixMultiplyTranspose(D3DXMATRIX* pOut, const D3DXMATRIX* pM1,
	const D3DXMATRIX* pM2)
{
	float* pA = (float*)pM1;
	float* pB = (float*)pM2;
	float pM[16];

	ZeroMemory(pM, sizeof(D3DMATRIX));
	for (WORD i = 0; i < 4; i++)
		for (WORD j = 0; j < 4; j++) 
			for (WORD k = 0; k < 4; k++) 
				pM[4 * j + i] += pA[4 * i + k] * pB[4 * k + j];
	memcpy(pOut, pM, sizeof(D3DMATRIX));
	return pOut;
}

/*
 * D3DXMatrixOrthoLH
 *
 * Builds a left-handed orthogonal projection matrix.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    w 
 *       Width of the view-volume. 
 *    h 
 *       Height of the view-volume. 
 *    zn 
 *       Minimum z-value of the view volume. 
 *    zf 
 *       Maximum z-value of the view volume. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a left-handed orthogonal
 *    projection matrix.
 * 
 * Remarks
 *    An orthogonal matrix is an invertible matrix for which the inverse
 *    of the matrix is equal to the transpose of the matrix. 
 * 
 *    All the parameters of the D3DXMatrixOrthoLH function are distances
 *    in camera-space. The parameters describe the dimensions of the
 *    view-volume.
 * 
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixOrthoLH function can be
 *    used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixOrthoLH(D3DXMATRIX* pOut, float w, float h,
	float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = 2.0f / w;
	pOut->_22 = 2.0f / h;
	pOut->_33 = 1.0f / (zf - zn);
	pOut->_43 = (-zn) / (zf - zn);
	pOut->_44 = 1.0f;
	return pOut;
}

/*
 * D3DXMatrixOrthoRH
 *
 * Builds a right-handed orthogonal projection matrix.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    w 
 *       Width of the view-volume. 
 *    h 
 *       Height of the view-volume. 
 *    zn 
 *       Minimum z-value of the view volume. 
 *    zf 
 *       Maximum z-value of the view volume. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a right-handed orthogonal
 *    projection matrix.
 * 
 * Remarks
 *    An orthogonal matrix is an invertible matrix for which the inverse
 *    of the matrix is equal to the transpose of the matrix. 
 * 
 *    All the parameters of the D3DXMatrixOrthoRH function are distances
 *    in camera-space. The parameters describe the dimensions of the
 *    view-volume.
 * 
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixOrthoRH function can be
 *    used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixOrthoRH(D3DXMATRIX* pOut, float w, float h,
	float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = 2.0f / w;
	pOut->_22 = 2.0f / h;
	pOut->_33 = -1.0f / (zf - zn);
	pOut->_43 = (-zn) / (zf - zn);
	pOut->_44 = 1.0f;
	return pOut;
}

/*
 * D3DXMatrixOrthoOffCenterLH
 *
 * Builds a customized, left-handed orthogonal projection matrix.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    l 
 *       Minimum x-value of view-volume. 
 *    r 
 *       Maximum x-value of view-volume. 
 *    t 
 *       Minimum y-value of view-volume. 
 *    b 
 *       Maximum y-value of view-volume. 
 *    zn 
 *       Minimum z-value of the view volume. 
 *    zf 
 *       Maximum z-value of the view volume. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a customized, left-handed
 *    orthogonal projection matrix.
 *
 * Remarks
 *    An orthogonal matrix is an invertible matrix for which the inverse
 *    of the matrix is equal to the transpose of the matrix. 
 *
 *    The D3DXMatrixOrthoLH function is a special case of the
 *    D3DXMatrixOrthoOffCenterLH function. To create the same projection
 *    using D3DXMatrixOrthoOffCenterLH, use the following values:
 *    l = -w/2, r = w/2, b = -h/2, and t = h/2.
 *
 *    All the parameters of the D3DXMatrixOrthoOffCenterLH function are
 *    distances in camera-space. The parameters describe the dimensions
 *    of the view-volume.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixOrthoOffCenterLH function
 *    can be used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixOrthoOffCenterLH(D3DXMATRIX* pOut,
	float l, float r, float t, float b, float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = 2.0f / (r - l);
	pOut->_22 = 2.0f / (b - t);
	pOut->_33 = 1.0f / (zf - zn);
	pOut->_41 = -(r + l) / (r - l);
	pOut->_42 = (t + b) / (t - b);
	pOut->_43 = (-zn) / (zf - zn);
	pOut->_44 = 1.0f;
	return pOut;
}

/*
 * D3DXMatrixOrthoOffCenterRH
 *
 * Builds a customized, right-handed orthogonal projection matrix.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    l 
 *       Minimum x-value of view-volume. 
 *    r 
 *       Maximum x-value of view-volume. 
 *    t 
 *       Minimum y-value of view-volume. 
 *    b 
 *       Maximum y-value of view-volume. 
 *    zn 
 *       Minimum z-value of the view volume. 
 *    zf 
 *       Maximum z-value of the view volume. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a customized, right-handed
 *    orthogonal projection matrix.
 *
 * Remarks
 *    An orthogonal matrix is an invertible matrix for which the inverse
 *    of the matrix is equal to the transpose of the matrix. 
 *
 *    The D3DXMatrixOrthoRH function is a special case of the
 *    D3DXMatrixOrthoOffCenterRH function. To create the same projection
 *    using D3DXMatrixOrthoOffCenterRH, use the following values:
 *    l = -w/2, r = w/2, b = -h/2, and t = h/2.
 *
 *    All the parameters of the D3DXMatrixOrthoOffCenterRH function are
 *    distances in camera-space. The parameters describe the dimensions
 *    of the view-volume.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixOrthoOffCenterRH function
 *    can be used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixOrthoOffCenterRH(D3DXMATRIX* pOut,
	float l, float r, float t, float b, float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = 2.0f / (r - l);
	pOut->_22 = 2.0f / (b - t);
	pOut->_33 = -1.0f / (zf - zn);
	pOut->_41 = -(r + l) / (r - l);
	pOut->_42 = (t + b) / (t - b);
	pOut->_43 = (-zn) / (zf - zn);
	pOut->_44 = 1.0f;
	return pOut;
}

/*
 * D3DXMatrixPerspectiveLH
 *
 * Builds a left-handed perspective projection matrix
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    w 
 *       Width of the view-volume at the near view-plane. 
 *    h 
 *       Height of the view-volume at the near view-plane. 
 *    zn 
 *       Z-value of the near view-plane. 
 *    zf 
 *       Z-value of the far view-plane. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a left-handed perspective
 *    projection matrix.
 *
 * Remarks
 *    All the parameters of the D3DXMatrixPerspectiveLH function are
 *    distances in camera-space. The parameters describe the dimensions
 *    of the view-volume.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixPerspectiveLH function can
 *    be used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixPerspectiveLH(D3DXMATRIX* pOut, float w, float h,
	float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = (2.0f * zn) / w;
	pOut->_22 = (2.0f * zn) / h;
	pOut->_33 = zf / (zf - zn);
	pOut->_34 = 1.0f;
	pOut->_43 = -(zf * zn) / (zf - zn);
	return pOut;
}

/*
 * D3DXMatrixPerspectiveRH
 *
 * Builds a right-handed perspective projection matrix
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    w 
 *       Width of the view-volume at the near view-plane. 
 *    h 
 *       Height of the view-volume at the near view-plane. 
 *    zn 
 *       Z-value of the near view-plane. 
 *    zf 
 *       Z-value of the far view-plane. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that is a right-handed perspective
 *    projection matrix.
 *
 * Remarks
 *    All the parameters of the D3DXMatrixPerspectiveRH function are
 *    distances in camera-space. The parameters describe the dimensions
 *    of the view-volume.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixPerspectiveRH function can
 *    be used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixPerspectiveRH(D3DXMATRIX* pOut, float w, float h,
	float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = 2.0f / w;
	pOut->_22 = 2.0f / h;
	pOut->_33 = -zf / (zf - zn);
	pOut->_34 = -1.0f;
	pOut->_43 = -(zf * zn) / (zf - zn);
	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixPerspectiveFovLH(D3DXMATRIX* pOut, float fovy,
	float Aspect, float zn, float zf)
{
	float w = (1.0f / tan(fovy / 2)) / Aspect;
	float h = 1.0f / tan(fovy / 2);
	float Q = zf / (zf - zn);

	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = w;
	pOut->_22 = h;
	pOut->_33 = Q;
	pOut->_34 = 1.0f;
	pOut->_43 = -Q * zn;

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixPerspectiveFovRH(D3DXMATRIX* pOut, float fovy,
	float Aspect, float zn, float zf)
{
	float w = (1.0f / tan(fovy / 2)) / Aspect;
	float h = 1.0f / tan(fovy / 2);
	float Q = zf / (zf - zn);

	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = w;
	pOut->_22 = h;
	pOut->_33 = -Q;
	pOut->_34 = -1.0f;
	pOut->_43 = -Q * zn;

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixPerspectiveOffCenterLH(D3DXMATRIX* pOut,
	float l, float r, float b, float t, float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = (2.0f * zn) / (r - l);
	pOut->_22 = (2.0f * zn) / (b - t);
	pOut->_31 = -(r + l) / (r - l);
	pOut->_32 = (t + b) / (t - b);
	pOut->_33 = zf / (zf - zn);
	pOut->_34 = 1.0f;
	pOut->_43 = -(zf * zn) / (zf - zn);
	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixPerspectiveOffCenterRH(D3DXMATRIX* pOut,
	float l, float r, float b, float t, float zn, float zf)
{
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = (2.0f * zn) / (r - l);
	pOut->_22 = (2.0f * zn) / (b - t);
	pOut->_31 = (r + l) / (r - l);
	pOut->_32 = -(t + b) / (t - b);
	pOut->_33 = -zf / (zf - zn);
	pOut->_34 = -1.0f;
	pOut->_43 = -(zf * zn) / (zf - zn);
	return pOut;
}

/*
 * D3DXMatrixReflect
 *
 * Builds a matrix that reflects the coordinate system about a plane.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pPlane 
 *       Pointer to the source D3DXPLANE structure. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that reflects the coordinate system
 *    about the source plane.
 *
 * Remarks
 *    This function normalizes the plane equation before it creates the
 *    reflected matrix.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixReflect function can be
 *    used as a parameter for another function.
 * 
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixReflect(D3DXMATRIX* pOut, CONST D3DXPLANE* pPlane)
{
	float q = pPlane->a * pPlane->a + pPlane->b * pPlane->b +
		pPlane->c * pPlane->c;

	pOut->_11 = (q - (2.0f * pPlane->a * pPlane->a)) / q;
	pOut->_12 = (-2.0f * pPlane->a * pPlane->b) / q;
	pOut->_13 = (-2.0f * pPlane->a * pPlane->c) / q;

	pOut->_21 = (-2.0f * pPlane->a * pPlane->b) / q;
	pOut->_22 = (q - (2.0f * pPlane->b * pPlane->b)) / q;
	pOut->_23 = (-2.0f * pPlane->b * pPlane->c) / q;

	pOut->_31 = (-2.0f * pPlane->a * pPlane->c) / q;
	pOut->_32 = (-2.0f * pPlane->b * pPlane->c) / q;
	pOut->_33 = (q - (2.0f * pPlane->c * pPlane->c)) / q;

	pOut->_41 = (-2.0f * pPlane->a * pPlane->d) / q;
	pOut->_42 = (-2.0f * pPlane->b * pPlane->d) / q;
	pOut->_43 = (-2.0f * pPlane->c * pPlane->d) / q;
   
	pOut->_14 = pOut->_24 = pOut->_34 = 0;
	pOut->_44 = 1.0f;

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixRotationAxis(D3DXMATRIX* pOut, CONST D3DXVECTOR3* pV,
	float Angle)
{
	// FIXME: Test
	register float fCos = static_cast<float>(cos(Angle));
	register float fSin = static_cast<float>(sin(Angle));
	D3DXVECTOR3 v;
	
	D3DXVec3Normalize(&v, pV);
#if defined(__GNUC__) && defined(__i386__)
	asm("fsincos" : "=t" (fCos), "=u" (fSin) : "0" (Angle));
#else
	fCos = static_cast<float>(cos(Angle));
	fSin = static_cast<float>(sin(Angle));
#endif
	pOut->_11 = (v.x * v.x) * (1.0f - fCos) + fCos;
	pOut->_12 = (v.x * v.y) * (1.0f - fCos) - (v.z * fSin);
	pOut->_13 = (v.x * v.z) * (1.0f - fCos) + (v.y * fSin);
	pOut->_21 = (v.y * v.x) * (1.0f - fCos) + (v.z * fSin);
	pOut->_22 = (v.y * v.y) * (1.0f - fCos) + fCos ;
	pOut->_23 = (v.y * v.z) * (1.0f - fCos) - (v.x * fSin);
	pOut->_31 = (v.z * v.x) * (1.0f - fCos) - (v.y * fSin);
	pOut->_32 = (v.z * v.y) * (1.0f - fCos) + (v.x * fSin);
	pOut->_33 = (v.z * v.z) * (1.0f - fCos) + fCos;
	pOut->_14 = pOut->_24 = pOut->_34 = 
	pOut->_41 = pOut->_42 = pOut->_43 = 0.0f;
	pOut->_44 = 1.0f;

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixRotationQuaternion(D3DXMATRIX* pOut,
	CONST D3DXQUATERNION* pQ)
{
	float xx = pQ->x * pQ->x; float yy = pQ->y * pQ->y;
	float zz = pQ->z * pQ->z; float xy = pQ->x * pQ->y;
	float xz = pQ->x * pQ->z; float yz = pQ->y * pQ->z;
	float wx = pQ->w * pQ->x; float wy = pQ->w * pQ->y;
	float wz = pQ->w * pQ->z;

	pOut->_11 = 1 - 2 * (yy + zz); 
	pOut->_12 = 2 * (xy - wz);
	pOut->_13 = 2 * (xz + wy);

	pOut->_21 = 2 * (xy + wz);
	pOut->_22 = 1 - 2 * (xx + zz);
	pOut->_23 = 2 * (yz - wx);

	pOut->_31 = 2 * (xz - wy);
	pOut->_32 = 2 * (yz + wx);
	pOut->_33 = 1 - 2 * (xx + yy);

	pOut->_14 = pOut->_24 = pOut->_34 = 0.0f;
	pOut->_41 = pOut->_42 = pOut->_43 = 0.0f;
	pOut->_44 = 1.0f;

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixRotationYawPitchRoll(D3DXMATRIX* pOut,
	float Yaw, float Pitch, float Roll)
{
	float YawSin, YawCos, PitchSin, PitchCos, RollSin, RollCos;

#if defined(__GNUC__) && defined(__i386__)
	asm("fsincos" : "=t" (YawCos), "=u" (YawSin) : "0" (Yaw));
	asm("fsincos" : "=t" (PitchCos), "=u" (PitchSin) : "0" (Pitch));
	asm("fsincos" : "=t" (RollCos), "=u" (RollSin) : "0" (Roll));
#else
	YawCos = static_cast<float>(cos(Yaw));
	YawSin = static_cast<float>(sin(Yaw));
	PitchCos = static_cast<float>(cos(Pitch));
	PitchSin = static_cast<float>(sin(Pitch));
	RollCos = static_cast<float>(cos(Roll));
	RollSin = static_cast<float>(sin(Roll));
#endif
	pOut->_11 = (YawSin * PitchSin) * RollSin + YawCos * RollCos;
	pOut->_12 = PitchCos * RollSin;
	pOut->_13 = (YawCos * PitchSin) * RollSin - YawSin * RollCos;
	pOut->_14 = 0.0f;

	pOut->_21 = (YawSin * PitchSin) * RollCos - YawCos * RollSin;
	pOut->_22 = PitchCos * RollCos;
	pOut->_23 = (YawCos * PitchSin) * RollCos + YawSin * RollSin;
	pOut->_24 = 0.0f;

	pOut->_31 = YawSin * PitchCos;
	pOut->_32 = -PitchSin;
	pOut->_33 = YawCos * PitchCos;
	pOut->_34 = 0.0f;

	pOut->_41 = pOut->_42 = pOut->_43 = 0.0f;
	pOut->_44 = 1.0f;

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixRotationX(D3DXMATRIX* pOut, float angle)
{
#if DBG
    if(!pOut)
        return NULL;
#endif

	float sin, cos;
    sincosf(angle, sin, cos);  // Determine sin and cos of angle.

	pOut->_11 = 1.0f; pOut->_12 =  0.0f; pOut->_13 = 0.0f; pOut->_14 = 0.0f;
	pOut->_21 = 0.0f; pOut->_22 =  cos;  pOut->_23 = sin;  pOut->_24 = 0.0f;
	pOut->_31 = 0.0f; pOut->_32 = -sin;  pOut->_33 = cos;  pOut->_34 = 0.0f;
	pOut->_41 = 0.0f; pOut->_42 =  0.0f; pOut->_43 = 0.0f; pOut->_44 = 1.0f;

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixRotationY(D3DXMATRIX* pOut, float Angle)
{
	D3DXMatrixIdentity(pOut);
#if defined(__GNUC__) && defined(__i386__)
	asm("fsincos" : "=t" (pOut->_11), "=u" (pOut->_31) : "0" (Angle));
#else
	pOut->_11 = static_cast<float>(cos(Angle));
	pOut->_31 = static_cast<float>(sin(Angle));
#endif
	pOut->_13 = -pOut->_31;
	pOut->_33 = pOut->_11;
	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixRotationZ(D3DXMATRIX* pOut, float Angle)
{
	D3DXMatrixIdentity(pOut);
#if defined(__GNUC__) && defined(__i386__)
	asm("fsincos" : "=t" (pOut->_11), "=u" (pOut->_12) : "0" (Angle));
#else
	pOut->_11 = static_cast<float>(cos(Angle));
	pOut->_12 = static_cast<float>(sin(Angle));
#endif
	pOut->_21 = -pOut->_12;
	pOut->_22 = pOut->_11;
	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixScaling(D3DXMATRIX* pOut, float x, float y,
	float z)
{
	// FIXME: Test
	ZeroMemory(pOut, sizeof(D3DMATRIX));
	pOut->_11 = x; pOut->_22 = y; pOut->_33 = z; pOut->_44 = 1.0f;
	return pOut;
}

/*
 * D3DXMatrixShadow
 *
 * Builds a matrix that flattens geometry into a plane.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pLight 
 *       Pointer to a D3DXVECTOR4 structure describing the light's position. 
 *    pPlane 
 *       Pointer to the source D3DXPLANE structure. 
 *
 * Return Values
 *    Pointer to a D3DXMATRIX structure that flattens geometry into a plane.
 *
 * Remarks
 *    The D3DXMatrixShadow function flattens geometry into a plane, as if
 *    casting a shadow from a light.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixShadow function can be used
 *    as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixShadow(D3DXMATRIX* pOut, CONST D3DXVECTOR4* pLight,
	CONST D3DXPLANE* pPlane)
{
	D3DXPLANE p;
	float dot;

	D3DXPlaneNormalize(&p, pPlane);	
	dot = p.a * pLight->x + p.b * pLight->y + p.c * pLight->z + p.d * pLight->w;

	pOut->_11 = dot - p.a * pLight->x;
	pOut->_12 = -p.a * pLight->y;
	pOut->_13 = -p.a * pLight->z;
	pOut->_14 = -p.a * pLight->w;

	pOut->_21 = -p.b * pLight->x;
	pOut->_22 = dot - p.b * pLight->y;
	pOut->_23 = -p.b * pLight->z;
	pOut->_24 = -p.b * pLight->w;

	pOut->_31 = -p.c * pLight->x;
	pOut->_32 = -p.c * pLight->y;
	pOut->_33 = dot - p.c * pLight->z;
	pOut->_34 = -p.c * pLight->w;

	pOut->_41 = -p.d * pLight->x;
	pOut->_42 = -p.d * pLight->y;
	pOut->_43 = -p.d * pLight->z;
	pOut->_44 = dot - p.d * pLight->w;
	
	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixTransformation(D3DXMATRIX* pOut,
	CONST D3DXVECTOR3* pScalingCenter, CONST D3DXQUATERNION* pScalingRotation,
	CONST D3DXVECTOR3* pScaling, CONST D3DXVECTOR3* pRotationCenter,
	CONST D3DXQUATERNION* pRotation, CONST D3DXVECTOR3* pTranslation)
{
	// FIXME: Test and optimize
	D3DXMATRIX Result;
	D3DXMATRIX Temp;

	if (pScaling)
	{
		D3DXMATRIX TempT, TempR;

		if (pScalingCenter)
			Result *= -*D3DXMatrixTranslation(&TempT, pScalingCenter->x,
				pScalingCenter->y, pScalingCenter->z);
		if (pScalingRotation)
			Result *= -*D3DXMatrixRotationQuaternion(&TempR, pScalingRotation);
		Result *= *D3DXMatrixScaling(&Temp, pScaling->x, pScaling->y,
			pScaling->z);
		if (pScalingRotation)
			Result *= *TempR;
		if (pScalingCenter)
			Result *= *TempT;
	}
	if (pRotation)
	{
		D3DXMATRIX TempT;

		if (pRotationCenter)
			Result *= -*D3DXMatrixTranslation(&TempT, pRotationCenter->x,
				pRotationCenter->y, pRotationCenter->z);
		Result *= *D3DXMatrixRotationQuaternion(&Temp, pRotation);
		if (pRotationCenter)
			Result *= *TempT;
	}
	if (pTranslation)
		Result *= *D3DXMatrixTranslation(&Temp, pTranslation->x,
			pTranslation->y, pTranslation->z);

	return pOut;
}

/*
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixTranslation(D3DXMATRIX* pOut, float x, float y,
	float z)
{
	D3DXMatrixIdentity(pOut);
	pOut->_41 = x; pOut->_42 = y; pOut->_43 = z;
	return pOut;
}

/*
 * D3DXMatrixTranspose
 *
 * Returns the matrix transpose of a matrix.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXMATRIX structure that is the result of the
 *       operation. 
 *    pM 
 *       Pointer to the source D3DXMATRIX structure. 
 *
 * Return Values
 *    Pointer to the D3DXMATRIX structure that is the matrix transpose of
 *    the matrix.
 *
 * Remarks
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXMatrixTranspose function can be
 *    used as a parameter for another function.
 * 
 * Status
 *    Works
 *
 */

D3DXMATRIX* WINAPI D3DXMatrixTranspose(D3DXMATRIX* pOut, CONST D3DXMATRIX* pM)
{	
	*pOut = D3DXMATRIX(
		pM->_11, pM->_21, pM->_31, pM->_41,
		pM->_12, pM->_22, pM->_32, pM->_42,
		pM->_13, pM->_23, pM->_33, pM->_43,
		pM->_14, pM->_24, pM->_34, pM->_44
	);
	return pOut;
}

/*
 * D3DXPlaneNormalize
 *
 * Returns the normal of a plane.
 *
 * Parameters
 *    pOut 
 *       Pointer to the D3DXPLANE structure that is the result of the operation. 
 *    pP 
 *       Pointer to the source D3DXPLANE structure. 
 *
 * Return Values
 *    Pointer to a D3DXPLANE structure, representing the normal of the plane.
 *
 * Remarks
 *    D3DXPlaneNormalize normalizes a plane so that |a,b,c| == 1.
 *
 *    The return value for this function is the same value returned in the
 *    pOut parameter. In this way, the D3DXPlaneNormalize function can be
 *    used as a parameter for another function.
 *
 * Status
 *    Works
 *
 */

D3DXPLANE* WINAPI D3DXPlaneNormalize(D3DXPLANE* pOut, CONST D3DXPLANE* pP)
{
	float k =
		static_cast<float>(sqrt(pP->a * pP->a + pP->b * pP->b + pP->c * pP->c));
	pOut->a = pP->a / k;
	pOut->b = pP->b / k;
	pOut->c = pP->c / k;
	pOut->d = pP->d / k;
	return pOut;
}

/*
 *
 */

D3DXQUATERNION* WINAPI D3DXQuaternionMultiply(D3DXQUATERNION* pOut,
	CONST D3DXQUATERNION* pQ1, CONST D3DXQUATERNION* pQ2)
{
	float x = pQ1->x * pQ2->w + pQ1->y * pQ2->z - pQ1->z * pQ2->y + pQ1->w * pQ2->x;
	float y = -pQ1->x * pQ2->z + pQ1->y * pQ2->w + pQ1->z * pQ2->x + pQ1->w * pQ2->y;
	float z = pQ1->x * pQ2->y - pQ1->y * pQ2->x + pQ1->z * pQ2->w + pQ1->w * pQ2->z;
	float w = -pQ1->x * pQ2->x - pQ1->y * pQ2->y - pQ1->z * pQ2->z + pQ1->w * pQ2->w;
	pOut->x = x; pOut->y = y; pOut->z = z; pOut->w = w;
	return pOut;
}

/*
 *
 */

D3DXVECTOR3* WINAPI D3DXVec3Normalize(D3DXVECTOR3* pOut, CONST D3DXVECTOR3* pV)
{
	float Length;

	Length = D3DXVec3Length(pV);
	if (Length == 0)
		return pOut;
	Length = 1.0f / Length;
	pOut->x = pV->x * Length;
	pOut->y = pV->y * Length;
	pOut->z = pV->z * Length;
	return pOut;
}

};
